#!/usr/bin/env python3
import sys, time
from PyQt5.QtWidgets import (
    QApplication, QWidget, QVBoxLayout, QHBoxLayout, QLabel,
    QDoubleSpinBox, QPushButton, QMessageBox, QGroupBox
)
from PyQt5.QtCore import QTimer, Qt

import rclpy
from rclpy.node import Node

from rcl_interfaces.srv import SetParameters
from rcl_interfaces.msg import Parameter as ParamMsg, ParameterValue, ParameterType

# 与后端节点名一致
REMOTE_NODE = '/visual_control_node'
SERVICE_NAME = f'{REMOTE_NODE}/set_parameters'
CMD_TOPIC = 'visual_control_node/delta_twist_cmds'

# ====== Default parameter values ======
DEFAULT_KP_X = 2.4
DEFAULT_KI_X = 0.20
DEFAULT_KD_X = 0.14
DEFAULT_TARGET_DEPTH_M = 0.20
DEFAULT_ERROR_THRESHOLD_DEPTH_M = 0.02
DEFAULT_RAW_WIN_X = 10
DEFAULT_MED_WIN_X = 17

DEFAULT_KP = 0.0013
DEFAULT_KI = 0.0002
DEFAULT_KD = 0.0001
DEFAULT_ERROR_THRESHOLD_X = 6.0
DEFAULT_RAW_WIN_Y = 48
DEFAULT_MED_WIN_Y = 36

DEFAULT_KP_Z = 0.001
DEFAULT_KI_Z = 0.0001
DEFAULT_KD_Z = 0.0006
DEFAULT_ERROR_THRESHOLD_Y = 3.0
DEFAULT_RAW_WIN_Z = 15
DEFAULT_MED_WIN_Z = 15

DEFAULT_CONTROL_DT = 0.01

# 阶段参数默认值
DEFAULT_SMALL_FORWARD_SPEED = 0.08
DEFAULT_PHASE1_END = 6.0
DEFAULT_PHASE2_END = 9.9
DEFAULT_PHASE3_BEGIN = 9.9


def make_param(name: str, value, param_type: str = 'double') -> ParamMsg:
    pv = ParameterValue()
    pm = ParamMsg()
    pm.name = name
    if param_type == 'integer':
        pv.type = ParameterType.PARAMETER_INTEGER
        pv.integer_value = int(value)
    else:
        pv.type = ParameterType.PARAMETER_DOUBLE
        pv.double_value = float(value)
    pm.value = pv
    return pm


class PIDParamClient(Node):
    def __init__(self):
        super().__init__('pid_param_gui_client')
        self.cli = self.create_client(SetParameters, SERVICE_NAME)
        if not self.cli.wait_for_service(timeout_sec=5.0):
            self.get_logger().warn(f'未检测到参数服务 {SERVICE_NAME}，GUI 可打开，但应用时会失败。')


class PIDGui(QWidget):
    def __init__(self, ros_node: PIDParamClient):
        super().__init__()
        self.ros = ros_node
        self.setWindowTitle('IBVS 参数调节 GUI')
        self._sending = False
        self._last_future = None
        self._send_deadline = None  # 超时截止时间

        # ===== 顶层布局 =====
        root = QVBoxLayout()
        two_cols = QHBoxLayout()
        left_col  = QVBoxLayout()
        right_col = QVBoxLayout()

        # ====== X ======
        grp_x = QGroupBox("X (Depth loop)")
        lay_x = QVBoxLayout()
        self.kp_x = self._spin("Kp_x", 0.0, 10.0, 6, 0.0001, DEFAULT_KP_X)
        self.ki_x = self._spin("Ki_x", 0.0, 10.0, 6, 0.0001, DEFAULT_KI_X)
        self.kd_x = self._spin("Kd_x", 0.0, 10.0, 6, 0.0001, DEFAULT_KD_X)
        self.target_depth_m = self._spin("target_depth_m", 0.0, 5.0, 3, 0.001, DEFAULT_TARGET_DEPTH_M)
        self.err_th_depth = self._spin("error_threshold_depth_m", 0.0, 1.0, 3, 0.001, DEFAULT_ERROR_THRESHOLD_DEPTH_M)
        self.raw_win_x = self._spin("raw_window_x", 1, 200, 0, 1.0, DEFAULT_RAW_WIN_X)
        self.med_win_x = self._spin("med_window_x", 1, 200, 0, 1.0, DEFAULT_MED_WIN_X)
        for w in [self.kp_x, self.ki_x, self.kd_x, self.target_depth_m, self.err_th_depth, self.raw_win_x, self.med_win_x]:
            lay_x.addLayout(w)
        grp_x.setLayout(lay_x)
        left_col.addWidget(grp_x)

        # ====== Y ======
        grp_y = QGroupBox("Y (Image x error)")
        lay_y = QVBoxLayout()
        self.kp = self._spin("Y Kp", 0.0, 10.0, 6, 0.0001, DEFAULT_KP)
        self.ki = self._spin("Y Ki", 0.0, 10.0, 6, 0.0001, DEFAULT_KI)
        self.kd = self._spin("Y Kd", 0.0, 10.0, 6, 0.0001, DEFAULT_KD)
        self.err_th_x = self._spin("error_threshold_x", 0.0, 2000.0, 0, 1.0, DEFAULT_ERROR_THRESHOLD_X)
        self.raw_win_y = self._spin("raw_window_y", 1, 200, 0, 1.0, DEFAULT_RAW_WIN_Y)
        self.med_win_y = self._spin("med_window_y", 1, 200, 0, 1.0, DEFAULT_MED_WIN_Y)
        for w in [self.kp, self.ki, self.kd, self.err_th_x, self.raw_win_y, self.med_win_y]:
            lay_y.addLayout(w)
        grp_y.setLayout(lay_y)
        left_col.addWidget(grp_y)

        # ====== Z ======
        grp_z = QGroupBox("Z (Image y error)")
        lay_z = QVBoxLayout()
        self.kp_z = self._spin("Z Kp", 0.0, 10.0, 6, 0.0001, DEFAULT_KP_Z)
        self.ki_z = self._spin("Z Ki", 0.0, 10.0, 6, 0.0001, DEFAULT_KI_Z)
        self.kd_z = self._spin("Z Kd", 0.0, 10.0, 6, 0.0001, DEFAULT_KD_Z)
        self.err_th_y = self._spin("error_threshold_y", 0.0, 2000.0, 0, 1.0, DEFAULT_ERROR_THRESHOLD_Y)
        self.raw_win_z = self._spin("raw_window_z", 1, 200, 0, 1.0, DEFAULT_RAW_WIN_Z)
        self.med_win_z = self._spin("med_window_z", 1, 200, 0, 1.0, DEFAULT_MED_WIN_Z)
        for w in [self.kp_z, self.ki_z, self.kd_z, self.err_th_y, self.raw_win_z, self.med_win_z]:
            lay_z.addLayout(w)
        grp_z.setLayout(lay_z)
        right_col.addWidget(grp_z)

        # ====== General ======
        grp_g = QGroupBox("General")
        lay_g = QVBoxLayout()
        self.control_dt = self._spin("control_dt", 0.0001, 1.0, 4, 0.001, DEFAULT_CONTROL_DT)
        lay_g.addLayout(self.control_dt)
        self.small_forward_speed = self._spin("small_forward_speed", 0.0, 1.0, 4, 0.001, DEFAULT_SMALL_FORWARD_SPEED)
        self.phase1_end = self._spin("phase1_end", 0.0, 60.0, 2, 0.1, DEFAULT_PHASE1_END)
        self.phase2_end = self._spin("phase2_end", 0.0, 60.0, 2, 0.1, DEFAULT_PHASE2_END)
        self.phase3_begin = self._spin("phase3_begin", 0.0, 60.0, 2, 0.1, DEFAULT_PHASE3_BEGIN)
        for w in [self.small_forward_speed, self.phase1_end, self.phase2_end, self.phase3_begin]:
            lay_g.addLayout(w)
        grp_g.setLayout(lay_g)
        right_col.addWidget(grp_g)

        two_cols.addLayout(left_col, 1)
        two_cols.addLayout(right_col, 1)
        root.addLayout(two_cols)

        # ====== 按钮 ======
        btn_row = QHBoxLayout()
        self.btn_apply = QPushButton('手动应用全部')
        self.btn_apply.clicked.connect(self.on_apply_all_clicked)
        btn_row.addWidget(self.btn_apply)

        self.btn_apply_x = QPushButton('仅应用 X(深度)')
        self.btn_apply_x.clicked.connect(self.on_apply_x_only_clicked)
        btn_row.addWidget(self.btn_apply_x)

        root.addLayout(btn_row)

        # ====== 底部状态 ======
        self.status_lab = QLabel("状态：未发送")
        root.addWidget(self.status_lab)
        self.setLayout(root)

        # ===== rclpy 事件循环 =====
        self.spin_timer = QTimer(self)
        self.spin_timer.timeout.connect(self.spin_once)
        self.spin_timer.start(10)

        # ===== 自动下发 =====
        self.auto_timer = QTimer(self)
        self.auto_timer.timeout.connect(self.on_auto_tick)
        self.auto_timer.start(2000)

        # ===== 超时看门狗 =====
        self.watchdog = QTimer(self)
        self.watchdog.timeout.connect(self._check_timeout)
        self.watchdog.start(500)

    # ---------- widgets ----------
    def _spinbox(self, decimals, step, value, minimum, maximum):
        sb = QDoubleSpinBox()
        sb.setDecimals(decimals)
        sb.setSingleStep(step)
        sb.setValue(value)
        sb.setRange(minimum, maximum)
        return sb

    def _spin(self, label_text, min_v, max_v, decimals, step, default_v):
        layout = QHBoxLayout()
        lab = QLabel(label_text)
        sb = self._spinbox(decimals, step, default_v, min_v, max_v)
        layout.addWidget(lab)
        layout.addWidget(sb)
        layout.spin = sb
        return layout

    def spin_once(self):
        rclpy.spin_once(self.ros, timeout_sec=0.0)

    # ---------- collect ----------
    def _collect_all_params(self):
        return {
            'Kp_x': float(self.kp_x.spin.value()),
            'Ki_x': float(self.ki_x.spin.value()),
            'Kd_x': float(self.kd_x.spin.value()),
            'target_depth_m': float(self.target_depth_m.spin.value()),
            'error_threshold_depth_m': float(self.err_th_depth.spin.value()),
            'raw_window_x': int(self.raw_win_x.spin.value()),
            'med_window_x': int(self.med_win_x.spin.value()),
            'Kp': float(self.kp.spin.value()),
            'Ki': float(self.ki.spin.value()),
            'Kd': float(self.kd.spin.value()),
            'error_threshold_x': float(self.err_th_x.spin.value()),
            'raw_window_y': int(self.raw_win_y.spin.value()),
            'med_window_y': int(self.med_win_y.spin.value()),
            'Kp_z': float(self.kp_z.spin.value()),
            'Ki_z': float(self.ki_z.spin.value()),
            'Kd_z': float(self.kd_z.spin.value()),
            'error_threshold_y': float(self.err_th_y.spin.value()),
            'raw_window_z': int(self.raw_win_z.spin.value()),
            'med_window_z': int(self.med_win_z.spin.value()),
            'control_dt': float(self.control_dt.spin.value()),
            'small_forward_speed': float(self.small_forward_speed.spin.value()),
            'phase1_end': float(self.phase1_end.spin.value()),
            'phase2_end': float(self.phase2_end.spin.value()),
            'phase3_begin': float(self.phase3_begin.spin.value()),
        }

    def _collect_x_params(self):
        return {
            'Kp_x': float(self.kp_x.spin.value()),
            'Ki_x': float(self.ki_x.spin.value()),
            'Kd_x': float(self.kd_x.spin.value()),
            'target_depth_m': float(self.target_depth_m.spin.value()),
            'error_threshold_depth_m': float(self.err_th_depth.spin.value()),
            'raw_window_x': int(self.raw_win_x.spin.value()),
            'med_window_x': int(self.med_win_x.spin.value()),
        }

    # ---------- send ----------
    def _apply_params(self, params):
        if not self.ros.cli.service_is_ready():
            self._set_status("参数服务未就绪", ok=False)
            return
        if self._sending:
            self._set_status("上次未完成，跳过", ok=None)
            return

        ts = time.strftime('%H:%M:%S')
        print(f"[{ts}] 开始发送参数…")
        self._set_status(f"正在发送… {ts}", ok=None)

        req = SetParameters.Request()
        for k, v in params.items():
            if k.startswith('raw_window') or k.startswith('med_window'):
                req.parameters.append(make_param(k, int(v), 'integer'))
            else:
                req.parameters.append(make_param(k, float(v), 'double'))

        self._sending = True
        self._send_deadline = time.time() + 3.0  # 3 秒超时
        future = self.ros.cli.call_async(req)
        self._last_future = future

        def done_cb(fut):
            if not self._sending:  # 已超时丢弃
                return
            self._sending = False
            ts2 = time.strftime('%H:%M:%S')
            try:
                resp = fut.result()
                ok = all(r.successful for r in resp.results)
                if ok:
                    print(f"[{ts2}] 发送成功")
                    self._set_status(f"发送成功 {ts2}", ok=True)
                else:
                    print(f"[{ts2}] 发送失败")
                    self._set_status(f"发送失败 {ts2}", ok=False)
            except Exception as e:
                print(f"[{ts2}] 异常: {e}")
                self._set_status(f"异常 {ts2}: {e}", ok=False)

        future.add_done_callback(lambda f: done_cb(f))

    def _check_timeout(self):
        if self._sending and self._send_deadline and time.time() > self._send_deadline:
            ts = time.strftime('%H:%M:%S')
            print(f"[{ts}] 超过3秒未响应，作废本次发送")
            self._sending = False
            self._last_future = None
            self._send_deadline = None
            self._set_status("超时已重置", ok=False)

    def _set_status(self, text, ok: bool | None):
        if ok is True:
            self.status_lab.setStyleSheet("color: #2e7d32;")
        elif ok is False:
            self.status_lab.setStyleSheet("color: #c62828;")
        else:
            self.status_lab.setStyleSheet("")
        self.status_lab.setText(f"状态：{text}")

    # ---------- auto ----------
    def on_auto_tick(self):
        if not self._is_ibvs_alive():
            self._set_status("IBVS 未运行，跳过", ok=None)
            return
        self._apply_params(self._collect_all_params())

    def on_apply_all_clicked(self):
        if not self._is_ibvs_alive():
            QMessageBox.warning(self, '提示', 'IBVS 未运行')
            return
        self._apply_params(self._collect_all_params())

    def on_apply_x_only_clicked(self):
        if not self._is_ibvs_alive():
            QMessageBox.warning(self, '提示', 'IBVS 未运行')
            return
        self._apply_params(self._collect_x_params())

    def _is_ibvs_alive(self) -> bool:
        return self.ros.cli.service_is_ready()


def main():
    rclpy.init()
    node = PIDParamClient()
    app = QApplication(sys.argv)
    gui = PIDGui(node)
    gui.resize(900, 700)
    gui.show()
    code = app.exec_()
    node.destroy_node()
    rclpy.shutdown()
    sys.exit(code)


if __name__ == '__main__':
    main()
